/**
 * @file CmdGenerator.cpp
 * @author Huysmans Bjorn
 * @brief Definition of this class, declared in CmdGenerator.h
 */

#include "CmdGenerator.h"
#include "Point.h"
#include "math.h"
#include <cmath>
#include <vector>
#include <stdio.h>
#include "Cmd.h"

#define CmdGen_PI 3.14159265

using namespace std;

CmdGenerator::CmdGenerator(){
	// Constructor
}

CmdGenerator::~CmdGenerator(){
	// Destructor
}

vector<Cmd> CmdGenerator::generateList(vector<Point> CoordinatesList, int initialAngle){
	int quadrant;

	printf("Allocating commandlist, size: %d\n", CoordinatesList.size());
	vector<Cmd> CmdList(CoordinatesList.size()-1);
	int currentAngle = initialAngle;
	int previousQuadrant = determineQuadrant(currentAngle);
	printf("Cmdlist allocated\n");
	// Loop as long the list is -1 because last point is end point
	for(int i = 0; i < CoordinatesList.size()-1; i++)
	{
		int nextAngle, turn, distance;

		// Determine quadrant
		if(CoordinatesList.at(i).getX() > CoordinatesList.at(i+1).getX())
		{
			if(CoordinatesList.at(i).getY() < CoordinatesList.at(i+1).getY())
					quadrant = 3;
			else
				quadrant = 2;
		}
		else
		{
			if(CoordinatesList.at(i).getY() < CoordinatesList.at(i+1).getY())
				quadrant = 4;
			else
				quadrant = 1;
		}
		
		// Calculate new end angle
		nextAngle = calculateAngle(CoordinatesList.at(i), CoordinatesList.at(i+1), quadrant);

		// Calculate turning angle
		turn = calculateTurn(currentAngle, nextAngle);

		printf("Curr angle: %d, Next angle: %d, Turn: %d\n", currentAngle, nextAngle, turn);

		// Set new currentAngle
		currentAngle = nextAngle;
		
		// Set new previous quadrant
		previousQuadrant = quadrant;

		// Calculate distance
		distance = calculateDistance(CoordinatesList.at(i), CoordinatesList.at(i+1));

		// Add to the command list
		Cmd cmdTemp (turn,distance);
		CmdList.at(i) = cmdTemp;
		printf("CmdList %d: turn = %d, distance = %lf\n", i, CmdList.at(i).getAngle(), CmdList.at(i).getDistance());

	}
	return CmdList;
}

int CmdGenerator::calculateTurn(int currentAngle, int endAngle){
	int turn;

	turn = 180 - abs(abs(currentAngle - endAngle) - 180);

	printf("CurrA: %d, endA: %d\n", currentAngle, endAngle);

	if(endAngle > currentAngle)
	{
		if(abs(endAngle - currentAngle) < 180)
			turn = turn * (-1);
	}
	else
	{
		if(abs(endAngle - currentAngle) > 180)
			turn = turn * (-1);
	}


	return turn;
}

int CmdGenerator::determineQuadrant(int angle){
	int quadrant;

	quadrant = angle % 90;

	return quadrant;
}


int CmdGenerator::calculateAngle(Point p1, Point p2, int quadrant){
	// Calculates the angle using Pythagoras
	double a, b, angle;

	a = (p1.getX() - p2.getX());
	b = (p1.getY() - p2.getY());

	// Angle with horizontal reference
	angle = atan(b/a) * (180 / CmdGen_PI);

	// Compensate with quadrants
	//printf("Quadrant: %d\n", quadrant);
	switch (quadrant){
		case 1: angle = angle * (-1); break;
		case 2: angle = 180 - angle; break;
		case 3: angle = 180 - angle; break;
		case 4: angle = (360 - angle); break;
	}
	return (int)round(angle);
}

int CmdGenerator::calculateDistance(Point p1, Point p2){
	// Calculates the distance using Pythagoras
	int a, b, c;

	a = (p1.getX() - p2.getX()) * (p1.getX() - p2.getX());
	b = (p1.getY() - p2.getY()) * (p1.getY() - p2.getY());
	c = a * b;
	c = sqrt(c);
	return c;
}
